#pragma once
#ifndef IMM_UKF_HPP_
#define IMM_UKF_HPP_

#include "Airplane.hpp"

#include "UKF.hpp"

#include <vector>
// #include <string>
// #include <Eigen/Dense>
#include "iostream"
#include "fstream"
#include "sstream"

extern double data1[];

extern Eigen::Map<Eigen::Matrix<double, 3, 3>> temp_t;
extern const Eigen::MatrixXd default_interact_pro;
extern Eigen::Vector3d	temp_q;
extern const Eigen::VectorXd default_model_pro;
extern double minT;
extern double data2[];
extern Eigen::Map<Eigen::Matrix<double,3, 10>> Qtemp;
extern const Eigen::MatrixXd default_Q;
extern const std::vector<std::string> default_model;


/**
 * @brief 根据多台飞机获得的目标图像，计算目标运动状态
 * 
 * @param 初次输入:
 * 			S2计算所得飞机初始位置
 * 			S2量测的时间均值
 * 			每个模型的初始协方差P0 （内置默认值）
 * 			每个飞机的测量噪声Ri, 存在飞机内
 * 			每个模型的过程噪声Qj（内置默认值）
 * 			每个模型的概率model_pro_（默认初始均为1/3）
 * 			一步转移概率矩阵interact_pro_ （内置默认值）
 * @param 每步输入:(融合成多个MeasureMents_In_Period，根据时间升序排列)
 * 			该时间段的量测时间均值
 * 			该时间段所有飞机的量测数据集合 std::map<uint64_t,<std::vector<Measurement_Pair>> Measurements
 * 			该时间段融合所得的R矩阵
 * @result 输出Filtered_Target_State
 */
class IMM_UKF{

public:
	//IMM_UKF(std::vector<std::string>& model, int state_v, int mea_v, Eigen::MatrixXd P);

	//IMM_UKF(std::vector<std::string>& model, int state_v, Eigen::MatrixXd P);

	IMM_UKF(Eigen::MatrixXd interact_pro = default_interact_pro, Eigen::VectorXd model_pro = default_model_pro, int state_v = 10, std::vector<std::string> model = default_model);
	// 析构函数
    ~IMM_UKF(){
		//std::cout<<"xigou"<<std::endl;
	};
	
	/**
	 * @brief UKF发散或目标丢失时间超过限度后，
	 * 		丢弃所有已有数据，初始化状态重置
	 */
	void TargetLost(){isinitialized = false;};

	// // 进行imm ukf的初始化操作
	// void IMM_Initialization(Eigen::VectorXd& Z, float time, float velo, float angles);
	// void IMM_Initialization(std::vector<float> positon, Eigen::VectorXd& Z, float time, std::vector<float> vel,std::vector<float> acc, float angles);
	
	// 初始化需要将s2的结果集合为UKF的Filtered_Target_State Initial_State
	// 再将集合结果输入所有的UKF中
	bool IMM_Initialization(Eigen::Vector3d TargetPosition, uint64_t MeanTime, Eigen::VectorXd model_pro = default_model_pro, Eigen::MatrixXd P = default_P, Eigen::MatrixXd Q = default_Q);

	bool IsInitialized(){
		return isinitialized;
	}

	// 处理按时间块分割的量测数据
	bool Process(Filtered_Target_State &Merged_UKF_Output, std::vector<MeasureMents_In_Period> UKF_Inputs);

	//// 外部可用参数设置接口
	// 算法运行过程中需要缓存设定
	bool Set_model_P_(std::string model, Eigen::MatrixXd model_P);

	bool Set_model_Q_(std::string model, Eigen::MatrixXd model_Q);

	bool Set_model_pro_(std::string model, float modelP);

	bool Set_interact_pro_(Eigen::MatrixXd interact_pro);

	Filtered_Target_State Get_Merged_UKF_Output_(){return Merged_UKF_Output_;};


	// ////
	// // 初始化交互
	// void InputInteract();

	// // 预测测量值
	// void PredictionZmerge(float time);
	// // 更新概率
	// void UpdateProbability(std::vector<Eigen::VectorXd>& Z, const Eigen::VectorXd& beta, const float& last_beta);
	// // 融合概率
	// void UpdateProbability(Eigen::VectorXd& Z);
	// // 融合概率
	// void MixProbability();
	// // 融合状态
	// void Process(std::vector<Eigen::VectorXd>& Z, const Eigen::VectorXd& beta, const float& last_beta, float& time);

	// void Process(Eigen::VectorXd X_state,Eigen::VectorXd&Z,  float& time);
	

	// // 融合状态
	// Eigen::VectorXd getMixState();
	// // 融合协方差
	// Eigen::MatrixXd getMixCovariance();
	
	// Eigen::MatrixXd GetS();

	// Eigen::VectorXd GetZpre();

	// double calculate_likelihood(const Eigen::VectorXd &z, const Eigen::VectorXd &z_pred, const Eigen::MatrixXd &S);

private:

	// /**
	// * @brief 根据S2结果生成初始状态
	// * 
	// * 		拓展向量长度,生成对应结构体
	// *		结果同时分配给所有模型
	// */
	// bool GenerateInitialState(Filtered_Target_State &Output, 
	// 						  Eigen::Vector3d TargetPosition, uint64_t MeanTime);

	/**
	 * @brief 模型条件重初始化
	 * 		根据模型概率混合计算各模型的状态初值，用于输入UKF
	 * 		输入：模型
	 * 			该模型上次UKF的估计结果 X, P(存在std::vector<UKF> imm_ukf_里)
	 * 			每个模型的概率model_pro_（默认初始均为1/3）
	 *  		一步转移概率矩阵interact_pro_ （内置默认值）
	 * 		输出：重估计的X，P， 分别输入各个ukf
	 */
	// // 所有模型的重初始化 第一步
	// bool States_ReInitialization(std::vector<Eigen::VectorXd> &X, std::vector<Eigen::MatrixXd> &P,
	// 							 Eigen::MatrixXd interact_pro, Eigen::VectorXd model_pro);

	// 单个模型的重初始化
	bool State_ReInitialization(std::vector<Eigen::VectorXd> &X, std::vector<Eigen::MatrixXd> &P,
								Eigen::MatrixXd interact_pro, Eigen::VectorXd model_pro);
							  
	/**
	 * @brief 利用似然函数更新各模型概率 第三步
	 * 		  输出结果保存至成员Estimate_Result_
	 */
	bool UpdateMixState(Filtered_Target_State &Merged_UKF_Output, Eigen::VectorXd &model_pro,
						std::vector<Filtered_Target_State> UKF_Outputs, Eigen::VectorXd likelihood);					


	uint8_t id_;//当前该对象所处理的目标id

	uint64_t stdTime_;//滤波基准时间，为当前目标量测结果中的最晚量测时间

	// 多模型融合结果
	Filtered_Target_State IMM_UKF_Output_;

	int model_size_ = 0;//运动模型数量
	int n_x_ = 10;
	int n_z_ = 3;

	bool isinitialized = false;
	//按模型顺序排列
	std::vector<UKF> imm_ukf_;//不同模型的ukf
	std::vector<Eigen::VectorXd> model_X_;// 单个模型的状态初值
	std::vector<Eigen::MatrixXd> model_P_;

	std::vector<Eigen::VectorXd> X_hat_;// 单个模型的状态估计
	std::vector<Eigen::MatrixXd> P_hat_;
	Eigen::MatrixXd R_merge_;//所有模型共享同一个值

	float pi_ = 3.1415926;
	Eigen::MatrixXd interact_pro_;//模型一步转移概率矩阵
	Eigen::VectorXd model_pro_;//模型概率
	Eigen::VectorXd x_merge_; //融合状态向量
	Eigen::MatrixXd p_merge_;// 融合协方差
	Eigen::VectorXd c_; //归一化常数

	Eigen::MatrixXd S_merge_;//pzz
	Eigen::VectorXd Zpre_merge_;//

	// 按时间小->大顺序排列
	// 当前迭代正在处理的数据集
	std::vector<MeasureMents_In_Period> UKF_Inputs_;

	// 按模型顺序排列
	// 上次UKF输出
	Filtered_Target_State Merged_UKF_Output_;

	std::ofstream filewrite;
};

#endif